
from decimal import *

#getcontext().prec = 12

class PIDController:
    def __init__(self):
        self.CLASSIC_PID = 0
        self.OVERSHOOT = 1
        self.TYLU_PID = 2
        self.TEST_PID= 3

        self.actual = Decimal(0)
        self.target = Decimal(0)
        self.error = Decimal(0)
        self.previous_error = Decimal(0)
        self.output = Decimal(0)
        self.integral = Decimal(0)
        self.derivative = Decimal(0)
        self.dt = Decimal('2')
        self.ziegler_nicholas_tune(Decimal('0.75'), Decimal(2) , self.TYLU_PID)

    def ziegler_nicholas_tune(self, ku, tu, control_type = 0):
        if control_type == self.CLASSIC_PID:
            self.kp = Decimal('0.6') * ku
            self.ki = (Decimal('2.0') * self.kp) / tu
            self.kd = (self.kp * tu) / Decimal(8)
        if control_type == self.OVERSHOOT:
            self.kp = Decimal('0.33') * ku
            self.ki = (Decimal('2.0') * self.kp ) / tu
            self.kd = (self.kp * tu) / Decimal('3.0')
        if control_type == self.TEST_PID:
            self.kp = Decimal('0.5') * ku
            self.ki = 0
            self.kd = 0
        if control_type == self.TYLU_PID:
            self.kp = ku / Decimal('2.2')
            self.ki = Decimal('2.2') * tu
            self.kd = tu / Decimal('6.3')

    def set_actual(self, actual):
        self.actual = Decimal(str(actual))

    def set_target(self, target):
        self.target = Decimal(str(target))

    def get_output(self):
        return self.output

    def compute_prev_error(self):
        self.previous_error = self.target - self.actual

    def reset(self):
        self.error = Decimal(0)
        self.previous_error = Decimal(0)
        self.integral = Decimal(0)
        self.derivative = Decimal(0)
        self.output = Decimal(0)
        self.actual = Decimal(0)
        self.target = Decimal(0)

    def update(self, actual):
        self.actual = Decimal(str(actual))
        self.error = self.target - self.actual
        if self.error > Decimal(2) or self.error < Decimal(-2):
            self.error = Decimal(str(self.error / Decimal(str(2.0))))

        self.integral += self.error * self.dt
        self.derivative = (self.error - self.previous_error) / self.dt
        self.output = (self.kp * self.error) + (self.ki * self.integral) + (self.kd * self.derivative)
        self.previous_error = self.error

